#!/bin/bash
PATH=/bin:/sbin:/usr/bin:/usr/sbin:/usr/local/bin:/usr/local/sbin:~/bin
export PATH
GOLO_Ver='0.3'
. /opt/openresty/nginx/scripts/golo.conf
source $ROS_PATH
source $ROS_GOLO_WS_PATH
export ROS_IP=$ROSIP
rosparam set /ecu_swith true
ecucost_teb=$(rosparam get /move_base/base_global_planner |grep test_planner |wc -l)
if [ $ecucost_teb -ne 1 ];then
rosrun dynamic_reconfigure dynparam load /move_base $GOLO_WS_PATH/golo/golo_nav/src/$teb &
fi
while true
do
ecuswith=`rosparam get /ecu_swith`
if [ $ecuswith == true ];then
 break
fi
if [ $ecuswith != true ];then
 rosparam set /ecu_swith true
fi
done
if [ $ecuswith != true ];then
	 rosparam set /ecu_swith true
fi
sleep 0.05
if [ $ecucost_teb -ne 1 ];then
	rosrun dynamic_reconfigure dynparam load /move_base $GOLO_WS_PATH/golo/golo_nav/src/$teb &
fi
sleep 0.1
if [ $ecuswith == true ];then
rosnode kill /null_scan_relay
rosnode kill /scan_multi_merged
roslaunch merged_percept null_scan_relay.launch  &
fi
init_times=1
while true
do
    if [ $init_times -ge 10 ];then
        break
    fi
rosservice call /move_base/clear_costmaps
ecucost_data=$(rosparam get /move_base/TebLocalPlannerROS/penalty_epsilon |grep 0.1 |wc -l)
ecucost_teb=$(rosparam get /move_base/base_global_planner |grep test_planner |wc -l)
#if [  $ecucost_data -ne 1 ];then
#rosrun dynamic_reconfigure dynparam load /move_base/TebLocalPlannerROS $GOLO_WS_PATH/golo/golo_nav/src/$fristspeed &
#fi
ecuswith=`rosparam get /ecu_swith`
if [ $ecuswith != true ];then
   rosparam set /ecu_swith true
fi

if [ $ecucost_teb -eq 1 ] && [ $ecucost_data -ne 1 ] && [ $ecuswith == true ];then
    rosrun dynamic_reconfigure dynparam load /move_base/TebLocalPlannerROS $GOLO_WS_PATH/golo/golo_nav/src/$fristspeed &
    #echo $(date "+%H:%M:%S") >> /tmp/2023.txt
    #sleep 0.2
fi
if [  $ecucost_data -ne 1 ];then
rosrun dynamic_reconfigure dynparam load /move_base/TebLocalPlannerROS $GOLO_WS_PATH/golo/golo_nav/src/$fristspeed &
fi
init_times=$(($init_times+1))
sleep 0.1
done


